/**
  * @file     	pid.c
  * @author   	YangHaifeng
  * @email   	2995339193@qq.com
  * @version	V0.0
  * @license  	GNU General Public License (GPL)  
  * @detail		please follow the license!!!
  * @attention
  *  this file is aim to do something with pid									
  *   																	
  * @htmlonly 
  * <span style="font-weight: bold">History</span> 
  * @endhtmlonly 
  * Version|Auther|Describe
  * ------|----|-------- 
  * V0.0|YangHaifeng|Create File
  * <h2><center>&copy;COPYRIGHT YangHaifeng All Rights Reserved.</center></h2>
  */ 
  
#include "pid.h"



/**
 * @brief 增量式PID计算
 * @param None
 * @retval None
 * @author YungHaifeng
 */
float pid_inc(float target,float value)
{
	static float kp=10,ki=10,kd=0;
	static float last_err,prev_err;
	static float last_out;
	float d_out;
	float err;
	
	err = target - value;
	
	d_out = kp*(err - last_err) + ki*err + kd*(err - 2*last_err + prev_err);
	last_out += d_out;
	
	prev_err = last_err;
	last_err = err;
	
	return last_out;
}



/**
 * @brief 巡线PID计算
 * @param tar 目标值
 * @param val 实际值
 * @retval None
 * @author YungHaifeng
 */
float track_30_pid(float tar,float val)
{
	const static float kp=-15,kd=-25;
	static float err_last = 0;
	float err,out;
	
	err = tar - val;
	
	out = kp*err + kd*(err - err_last);
	
	err_last = err;
	
	return out;	
}





/**
 * @brief 巡线PID计算
 * @param tar 目标值
 * @param val 实际值
 * @retval None
 * @author YungHaifeng
 */
float track_50_pid(float tar,float val)
{
	const static float kp=-12,kd=-50;
	static float err_last = 0;
	float err,out;
	
	err = tar - val;
	
	out = kp*err + kd*(err - err_last);
	
	err_last = err;
	
	return out;	
}







/**
 * @brief 巡线PID计算
 * @param tar 目标值
 * @param val 实际值
 * @retval None
 * @author YungHaifeng
 */
float track_70_pid(float tar,float val)
{
	const static float kp=-0,kd=-0;
	static float err_last = 0;
	float err,out;
	
	err = tar - val;
	
	out = kp*err + kd*(err - err_last);
	
	err_last = err;
	
	return out;	
}





